#!/bin/bash
###
 # @Author: Lai Jun
 # @E-mail: laijun@nudt.edu.cn
 # @LastEditors: Lai Jun
 # @LastEditTime: 2021-12-07 09:26:40
### 
# setup XTDRONE with gitee connection
# this script assumes you already have ros and gazebo installed
# for more infomation, please refers to https://www.yuque.com/xtdrone/manual_cn/basic_config_1.11 and https://github.com/robin-shaun/XTDrone
# tested with ubuntu 18.04.
set -x
sudo apt-get install -y ninja-build exiftool python-argparse python-empy python-toml python-numpy python-yaml python-dev python-pip ninja-build protobuf-compiler libeigen3-dev genromfs xmlstarlet libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev
pip2 install pandas jinja2 pyserial cerberus pyulog numpy toml pyquaternion
pip3 install packaging numpy empy toml pyyaml jinja2
sudo apt-get install git build-essential 

WORK_DIR=$(pwd)

ROS_DIST=`ls /opt/ros` || echo "ros not installed in the default folder: /opt/ros, please make sure you have sourced it"
if [ -n $ROS_DIST ]
then
    source /opt/ros/$ROS_DIST/setup.sh || echo "soure error, do you use bash shell? or have your ros correctly installed?"
fi
echo "Your ros version is $(rosversion -d)"
ROS_DIST=$(rosversion -d)

ROS_PATH=$(which -a roscore)
if [ -z $ROS_PATH ]
then
    echo "can not find roscore, exit"
    exit 3
fi

ROS_PATH_BIN="$(dirname "$ROS_PATH")"
ROS_PATH_DIST="$(dirname "$ROS_PATH_BIN")"

#  setup mavros and mavros-extras in ./
echo "install mavros and mavros-extras"
sudo apt-get install ros-$ROS_DIST-mavros ros-$ROS_DIST-mavros-extras ros-$ROS_DIST-control-toolbox
wget -N https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh || (echo "can not download file, exit" && exit 4 )
sudo bash ./install_geographiclib_datasets.sh

# steup px4
git clone https://github.com/PX4/PX4-Autopilot.git PX4_Firmware
cd PX4_Firmware
# if in ubuntu 20.04 with gazebo 11, then choose the px4 version v1.12.0-beta5, it fixes some Gazebo 11 related issues.
px4tag=v1.11.0-beta1
if [[ $ROS_DIST == neotic ]];then
   px4tag=v1.12.0-beta5
fi
git checkout -b xtdrone/dev $px4tag
git submodule update --init --recursive
make px4_sitl_default
cd ..

# download XTDrone source

echo "download XTDrone source"
git clone https://github.com/robin-shaun/XTDrone.git
cd XTDrone
git submodule update --init --recursive
cp sensing/gimbal/gazebo_gimbal_controller_plugin.cpp $WORK_DIR/PX4_Firmware/Tools/sitl_gazebo/src/
cp sitl_config/init.d-posix/rcS $WORK_DIR/PX4_Firmware/ROMFS/px4fmu_common/init.d-posix/
cp sitl_config/worlds/* $WORK_DIR/PX4_Firmware/Tools/sitl_gazebo/worlds/
cp -r sitl_config/models/* $WORK_DIR/PX4_Firmware/Tools/sitl_gazebo/models/ 
# this following command will not run because the setupfile will 
# cp -r sitl_config/models/{3d_gpu_lidar,3d_lidar,hokuyo_lidar,kinect,stereo_camera} $WORK_DIR/.gazebo/models/ 
cp -r sitl_config/models/{3d_gpu_lidar,3d_lidar,hokuyo_lidar,kinect,stereo_camera} $HOME/.gazebo/models/
cp sitl_config/launch/* $WORK_DIR/PX4_Firmware/launch/ -r 
cd ../

# generate source file
echo "Generating source file.. "
SOURCE_STR="#THIS file is auto-generated by steup_xtdron_env.sh"$'\n'
#ros related
SOURCE_STR=$SOURCE_STR"source $ROS_PATH_DIST/setup.bash"$'\n'
SOURCE_STR=$SOURCE_STR"source $WORK_DIR/PX4_Firmware/Tools/setup_gazebo.bash $WORK_DIR/PX4_Firmware/ $WORK_DIR/PX4_Firmware/build/px4_sitl_default"
SOURCE_STR="$SOURCE_STR"$'\n'
SOURCE_STR=$SOURCE_STR"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$WORK_DIR/PX4_Firmware"$'\n'
SOURCE_STR=$SOURCE_STR"export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:$WORK_DIR/PX4_Firmware/Tools/sitl_gazebo"$'\n'
# gazebo related
SOURCE_STR=$SOURCE_STR"if [ -n \$GAZEBO_MODEL_PATH ]"$'\n'"then"$'\n'"export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:$WORK_DIR/XTDrone/sitl_config/models/"$'\n'"else"$'\n'"export GAZEBO_MODEL_PATH=$WORK_DIR/XTDrone/sitl_config/models/"$'\n'"fi"$'\n'"export GAZEBO_MODEL_DATABASE_URI=\"\""$'\n'
SOURCE_STR=$SOURCE_STR"export GAZEBO_MODEL_DATABASE_URI="$'\n'
SOUCE_NAME=source_setup_xt_env.bash
echo "$SOURCE_STR" > $SOUCE_NAME
echo "$SOUCE_NAME"' is ready, to setup your XTDrone environment, run the command' "\"source $SOUCE_NAME\""
echo 'NOTE: if you want to use gazebo to open world file in XTDrone/sitl_config/worlds, RUN' "\"source $SOUCE_NAME\" firstly."